{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2020-05-08T22:05:05.405241Z",
     "start_time": "2020-05-08T22:05:05.397090Z"
    }
   },
   "source": [
    "\n",
    "<div style=\"border:1px solid black; padding:20px 20px;text-align: justify;text-justify: inter-word\">\n",
    "    <strong>Exercise Session 5 - Path Planning<br/> Duration : 4 hours (2 in session + 2 at home)</strong><br/><br/>\n",
    "    <span style=\"text-decoration:underline;font-weight:bold;\">How to use this notebook?</span><br/>\n",
    "    This notebook is made of text cells and code cells. The code cells have to be <strong>executed</strong> to see the result of the program. To execute a cell, simply select it and click on the \"play\" button (<span style=\"font: bold 12px/30px Arial, serif;\">&#9658;</span>) in the tool bar just above the notebook, or type <code>shift + enter</code>. It is important to execute the code cells in their order of appearance in the notebook.<br/>\n",
    "You can make use of the table of contents to navigate easily between sections.\n",
    "</div>\n",
    "\n",
    "<br/>\n",
    "\n",
    "<div style=\"justify;text-justify: inter-word\">\n",
    "So that you may familiarise with the notebooks and the basic python syntax, the exercises are provided in notebook form and whenever there are any calculations to be made, we encourage you to do them by code. Also, if you want to take notes, we encourage you to use the markdown or Raw NBConvert cells. \n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "\n",
    "# Learning Goals\n",
    "\n",
    "- Understand path planning algorithms conceptually and on paper.\n",
    "- Implement and compare path planning algorithms (A* and Dijkstra) in terms of efficiency and performance.\n",
    "\n",
    "# Requirements\n",
    "\n",
    "- The printed ``grid1.pdf`` on paper. \n",
    "\n",
    "<img src=\"Images/grid1.png\" alt=\"gradient\" width=\"300\"/>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Understanding Path Planning on **Paper**\n",
    "\n",
    "For the paper based algorithms, you will need the files located in the Paper folder : \n",
    "\n",
    "- ``grid1.pdf` for the Dijkstra and A* algorithms on paper. \n",
    "\n",
    "\n",
    "## Dijkstra's Algorithm\n",
    "\n",
    "Apply the Dijkstra’s algorithm on **paper** on the map of the Figure below using a Manhattan distance.\n",
    "\n",
    "<img src=\"Images/grid1.png\" alt=\"gradient\" width=\"300\"/>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Understanding Path Planning on Paper\n",
    "\n",
    "For the paper based algorithms, you will need the files located in the Paper folder : \n",
    "\n",
    "- ``grid1.pdf`` or `grid-path-planning.xlsx` for the Dijkstra and A* algorithms depending on whether you prefer working on a spreadsheet or on paper. \n",
    "\n",
    "\n",
    "## Introduction to path planning\n",
    "\n",
    "Once a robot has a map, it is essential to be able to plan a path to go from one point to another. The techniques used to find the best path depend on the type of map and of the type of path that one wants to obtain. In many cases, the maps are transformed into graphs and the path search is transformed in a graph search.\n",
    "\n",
    "\n",
    "## Dijkstra's Algorithm\n",
    "\n",
    "Apply the Dijkstra’s algorithm on paper on the map of the Figure below using a Manhattan distance.\n",
    "\n",
    "\n",
    "<img src=\"Images/grid1.png\" alt=\"gradient\" width=\"300\"/>\n",
    "\n",
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "> The solution is provided in the image below. <br>\n",
    "<img src=\"Images/grid-plan.png\" alt=\"gradient\" width=\"600\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A* Algorithm\n",
    "\n",
    "Apply the A* algorithm on paper on the map of the previous exercise using a Manhattan distance from the source for the motion cost and the Manhattan distance to the goal (without obstacles) as heuristic function. The same excel file as for the previous exercice can be used here.\n",
    "\n",
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "> The solution for the <strong> extra </strong> exercise is given in the image below, presenting both the cost function and the heuristic function and their sum. The A* algorithm allows to reduce the number of evaluated cell, at a cost of a higher computation for each cell. If the heuristic function is simple the A* algorithm is more efficient than Dijkstra’s algorithm. <br><br>\n",
    "> a) Heuristic function. b) The first two iterations of the A algorithm\n",
    "\n",
    "\n",
    "<img src=\"images/astar_corr.png\" alt=\"gg\" width=\"600\"/>\n",
    "\n",
    "> c) The A* algorithm after 6 steps. d) The A* algorithm reaches the goal cell and finds a shortest path\n",
    "\n",
    "<img src=\"images/astar-corr_1.png\" alt=\"gg\" width=\"600\"/>\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "> If you want a reminder from the course slides of the pseudocode for the **Dijkstra algorithm with constant weights** (equivalent to Breadth-First Search), we have included it at the bottom of this exercise.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Implementation the Dijkstra algorithm (with weight)\n",
    "### Initial Map \n",
    "\n",
    "We first choose an environment for the grid map with <span style=\"color:green\"><strong>S  </strong></span> (Source/Start in green), <span style=\"color:red\">  <strong>G  </strong></span> (Goal in red), and <span style=\"color:black\"><strong>obstacles  </strong></span> (in black), as shown below. Morever we put weight on a cell (in <span style=\"color: yellow\"><strong>yellow</strong></span>) (representing areas that slow down movement along the path, ex: sand, etc.) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"Images/initial_map_with_cost.png\" alt=\"gradient\" width=\"200\"/>\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial Map (0: free, -1: obstacle, 2: weighted cell):\n",
      "[[-1. -1. -1. -1. -1. -1. -1. -1. -1.]\n",
      " [-1.  0.  0.  0.  0.  0.  0.  0. -1.]\n",
      " [-1.  0.  0.  0.  0.  0.  0.  0. -1.]\n",
      " [-1.  0.  0.  0.  0.  0.  0.  0. -1.]\n",
      " [-1.  0.  0.  0. -1. -1. -1. -1. -1.]\n",
      " [-1.  0.  0.  2. -1. -1. -1. -1. -1.]\n",
      " [-1.  0.  0.  0.  0.  0.  0.  0. -1.]\n",
      " [-1.  0.  0.  0.  0.  0.  0.  0. -1.]\n",
      " [-1. -1.  0.  0.  0.  0.  0.  0. -1.]\n",
      " [-1. -1. -1.  0.  0.  0.  0.  0. -1.]\n",
      " [-1. -1. -1. -1. -1. -1. -1. -1. -1.]]\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6840 characters]",
      "text/plain": [
       "<Figure size 400x400 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "##############################################\n",
    "#do not modify this code\n",
    "##############################################\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib.colors import ListedColormap\n",
    "from heapq import heappush, heappop # used if you want to use heap queue for priority queue (easier to implement) but not compulsory\n",
    "\n",
    "def display_map(map_grid, path, start, goal, explored):\n",
    "    cmap = ListedColormap(['white', 'black', 'blue', 'green', 'red', 'grey', 'yellow'])\n",
    "    map_display = np.zeros_like(map_grid, dtype=object)\n",
    "\n",
    "    # Assign colors based on the map grid values\n",
    "    map_display[map_grid == -1] = 'black'  # Obstacles\n",
    "    map_display[map_grid == 0] = 'white'   # Free space\n",
    "\n",
    "    for position in explored:\n",
    "        if map_display[tuple(position)] == 'white':\n",
    "            map_display[tuple(position)] = 'grey'  # Explored cells\n",
    "\n",
    "    # Visualize the path\n",
    "    for position in path:\n",
    "        if map_display[position[0], position[1]] in ['white', 'grey']:\n",
    "            map_display[position[0], position[1]] = 'blue'  # Path\n",
    "\n",
    "    map_display[map_grid > 0] = 'yellow'  # Obstacles\n",
    "    map_display[start[0], start[1]] = 'green'  # Start\n",
    "    map_display[goal[0], goal[1]] = 'red'      # Goal\n",
    "\n",
    "    # Convert color names to numbers for plotting\n",
    "    color_mapping = {'white': 0, 'black': 1, 'blue': 2, 'green': 3, 'red': 4, 'grey': 5, 'yellow': 6}\n",
    "    map_numeric_display = np.vectorize(color_mapping.get)(map_display)\n",
    "    fig, ax = plt.subplots(figsize=(4, 4))\n",
    "    ax.imshow(map_numeric_display, cmap=cmap)\n",
    "    ax.set_xticks(np.arange(-0.5, map_grid.shape[1], 1), minor=True)\n",
    "    ax.set_yticks(np.arange(-0.5, map_grid.shape[0], 1), minor=True)\n",
    "    ax.grid(which='minor', color='gray', linestyle='-', linewidth=0.5)\n",
    "    ax.tick_params(which='both', bottom=False, left=False, labelbottom=False, labelleft=False)\n",
    "    ax.set_title(f'{ALGO} Visualization')\n",
    "    plt.show()\n",
    "##############################################\n",
    "#end of code not to modify\n",
    "##############################################\n",
    "Map = np.zeros((11, 9))\n",
    "\n",
    "Map[5, 3] = 2; # Weighted cell\n",
    "\n",
    "Map[0, :] = -1; Map[10, :] = -1; Map[:, 0] = -1; Map[:, 8] = -1\n",
    "Map[8, 1] = -1; Map[9, 1] = -1; Map[9, 2] = -1; Map[4:6, 4:8] = -1\n",
    "SearchStart = (8, 5) \n",
    "SearchGoal  = (2, 6)\n",
    "print(\"Initial Map (0: free, -1: obstacle, 2: weighted cell):\")\n",
    "print(Map)\n",
    "ALGO = \"Initial Map\"\n",
    "display_map(Map, [], SearchStart, SearchGoal, [])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "> We recommend starting with the **4-connected grid** (Manhattan distance) and **not** the 8-connected grid (which includes diagonals) for now.\n",
    "\n",
    "<img src=\"Images/Square_4_connectivity-1.png\" alt=\"gradient\" width=\"150\"/>\n",
    "<img src=\"Images/Square_8_connectivity.png\" alt=\"gradient\" width=\"150\"/>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Implement the dijkstra algorithm**. You can use the code the hinted code below. \n",
    "We choose to use a priority queue to always expand the node with the lowest cost first. \n",
    "We use heapq library. \n",
    "\n",
    "Suggestion initialise with \n",
    "- came_from = {}\n",
    "- g_costs = {S: 0}, \n",
    "- explored = set()\n",
    "\n",
    "- open_set = ...\n",
    "\n",
    "(What are all these variables ?)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "> - came_from → how we got there (for reconstructing the path)\n",
    "> - g_costs →   current shortest known distance to each cell <br>\n",
    "> - explored →  what we’ve already checked (for visualization / tracking)\n",
    "> - open_set →  what we still need to explore (sorted by cost)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2024-10-08T17:28:47.648719Z",
     "iopub.status.busy": "2024-10-08T17:28:47.646559Z",
     "iopub.status.idle": "2024-10-08T17:28:48.390412Z",
     "shell.execute_reply": "2024-10-08T17:28:48.390063Z",
     "shell.execute_reply.started": "2024-10-08T17:28:47.648633Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Dijkstra path length = 13 \n",
      " [(8, 5), (7, 5), (6, 5), (6, 4), (6, 3), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (2, 3), (2, 4), (2, 5), (2, 6)]\n",
      "length explored: 48\n",
      "Dijkstra visualization\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 7052 characters]",
      "text/plain": [
       "<Figure size 400x400 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A* path length = 13 \n",
      " [(8, 5), (7, 5), (6, 5), (6, 4), (6, 3), (6, 2), (5, 2), (4, 2), (3, 2), (2, 2), (2, 3), (2, 4), (2, 5), (2, 6)]\n",
      "length explored: 37\n",
      "A* visualization\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6048 characters]",
      "text/plain": [
       "<Figure size 400x400 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from heapq import heappush, heappop\n",
    "\n",
    "algo = \"A*\"  # Change to \"A*\" to test A* algorithm\n",
    "\n",
    "def grid_search(map_grid, S, G, algo=\"Dijkstra\"):\n",
    "    \n",
    "    ## initialize the varibales above\n",
    "    came_from = {}      # to reconstruct path\n",
    "    g_costs = {S: 0}    # cost from start to the cell\n",
    "    explored = set()    # to keep track of explored cells\n",
    "    operation_count = 0 # to count the number of operations\n",
    "    if algo == \"Dijkstra\":\n",
    "        open_set = [(0, S)]  # priority queue for Dijkstra (cost, position)\n",
    "        #heappush(open_set, (0, S))\n",
    "    if algo == \"A*\":\n",
    "        open_set = [(heuristic(S, G), 0, S)]  # priority queue for A* (f_cost, g_cost, position)\n",
    "        \n",
    "    while open_set: # G goal is unmarked\n",
    "        if algo == \"Dijkstra\":\n",
    "            current_g_cost, current_pos = heappop(open_set)\n",
    "        if algo == \"A*\":\n",
    "            current_f_cost, current_g_cost, current_pos = heappop(open_set)\n",
    "\n",
    "        explored.add(current_pos)\n",
    "        \n",
    "        if current_pos == G: # if G goal is marked\n",
    "            break\n",
    "\n",
    "        # Get neighbors (up, down, left, right)\n",
    "        neighbors = [\n",
    "            (current_pos[0]-1, current_pos[1]),  # Up\n",
    "            (current_pos[0]+1, current_pos[1]),  # Down\n",
    "            (current_pos[0], current_pos[1]-1),  # Left\n",
    "            (current_pos[0], current_pos[1]+1)   # Right\n",
    "        ]\n",
    "\n",
    "        for neighbor in neighbors: # for each neighbor of marked cells\n",
    "            # Check if neighbor is within bounds\n",
    "            if (0 <= neighbor[0] < map_grid.shape[0]) and (0 <= neighbor[1] < map_grid.shape[1]): #check if the neighbor is less than 1 and greater than 0\n",
    "                # Check if neighbor is not an obstacle\n",
    "                if map_grid[neighbor[0], neighbor[1]] != -1:\n",
    "                    tentative_g_cost = current_g_cost + 1  + map_grid[neighbor[0], neighbor[1]]\n",
    "\n",
    "                    if neighbor not in g_costs or tentative_g_cost < g_costs[neighbor]:\n",
    "                        g_costs[neighbor] = tentative_g_cost\n",
    "                        came_from[neighbor] = current_pos\n",
    "                        operation_count += 1 \n",
    "                        if algo == \"Dijkstra\":\n",
    "                            heappush(open_set, (tentative_g_cost, neighbor))\n",
    "\n",
    "                        if algo == \"A*\":\n",
    "                            f_cost = tentative_g_cost + heuristic(neighbor, G)\n",
    "                            heappush(open_set, (f_cost, tentative_g_cost, neighbor))\n",
    "                        \n",
    "    # Reconstruct path\n",
    "    if current_pos == G:\n",
    "        path = []\n",
    "        while current_pos != S:\n",
    "            path.append(current_pos)\n",
    "            current_pos = came_from[current_pos]\n",
    "        path.append(S)\n",
    "        path.reverse()\n",
    "        return path, explored, operation_count  # Return reversed path and explored cells\n",
    "    else:\n",
    "    # If we reach here, no path was found\n",
    "        return None, explored, operation_count\n",
    "\n",
    "##############################################\n",
    "#do not modify this code\n",
    "##############################################\n",
    "# Dijkstra\n",
    "path, explored, operation_count = grid_search(Map, SearchStart, SearchGoal, algo=\"Dijkstra\")\n",
    "if path:\n",
    "    print(\"Dijkstra path length =\", len(path)-1, \"\\n\", path)\n",
    "    print(\"length explored:\", len(explored))\n",
    "    print(\"Dijkstra visualization\")\n",
    "    ALGO = \"Dijkstra\"\n",
    "    display_map(Map, path, SearchStart, SearchGoal, explored)\n",
    "else:\n",
    "    print(\"No path found with Dijkstra\")\n",
    "\n",
    "# A*\n",
    "#uncomment the lines below to test A* algorithm\n",
    "if algo == \"A*\":\n",
    "    ALGO = \"A*\"\n",
    "    path, explored, operation_count = grid_search(Map, SearchStart, SearchGoal, algo=\"A*\")\n",
    "if path and algo == \"A*\":\n",
    "    print(\"A* path length =\", len(path)-1, \"\\n\", path)\n",
    "    print(\"length explored:\", len(explored))\n",
    "    print(\"A* visualization\")\n",
    "    display_map(Map, path, SearchStart, SearchGoal, explored)\n",
    "else:\n",
    "    print(\"No path found with A*\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Implemententing the A* Algorithm\n",
    "In the same way, let's implement the A* algorithm.\n",
    "\n",
    "> Note that the A* algorithm is an extension of the Dijkstra algorithm. \n",
    "- What is the main difference?\n",
    "\n",
    "**Reuse** the Dijksta alogrithm above and add the things for A* algorithm (remove some comments if needed).\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def heuristic(a, b):\n",
    "    # Implement the Manhattan distance heuristic\n",
    "    return abs(a[0] - b[0]) + abs(a[1] - b[1])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Comparison of Dijkstra's Algorithm and A* Algorithm\n",
    "\n",
    "Try different aspects and compare the results of the 2 algo in terms of path length, number of operations, and visited cells..\n",
    "\n",
    "- Different costs assigned to the cells (highlighted in yellow)\n",
    "- Alternative heuristic functions (such as Euclidean distance) in an 8-connected grid\n",
    "- Comparison of the number of operations and visited cells for each algorithm\n",
    "- what is the limits of A* ?\n",
    "\n",
    "<span style=\"color: #2980B9 ;\">\n",
    "\n",
    "\n",
    "- **Efficiency:** A* with an appropriate heuristic is generally more efficient than Dijkstra's Algorithm because it uses heuristics to guide the search towards the goal, reducing the number of visited cells.\n",
    "- **Heuristic Impact:** The choice of heuristic affects the performance of the A* algorithm.\n",
    "A heuristic that closely estimates the actual cost to reach the goal can significantly reduce the number of operations.\n",
    "- **Path Cost:** All algorithms find the optimal path in terms of total cost unless the heuristic used is **not admissible** (overestimates the cost).\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Recall of Dikstra Algorithm with constant weights (~ Breadth-First Search [BFS])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the pseudocode for the Dijkstra algorithm (Basic ~ Breaddth-First Search) : \n",
    "\n",
    "<img src=\"Images/pseudo.png\" alt=\"gradient\" width=\"450\"/>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\" pseudocode for Dijkstra's algorithm in text form\n",
    "\n",
    "integer n ← 0                  # Distance from start\n",
    "cell array grid ← all unmarked # Grid map\n",
    "cell list path ← empty         # Shortest path\n",
    "cell current                   # Current cell in path\n",
    "cell c                         # Index over cells\n",
    "cell S ← ...                   # Source cell\n",
    "cell G ← ...                   # Goal cell\n",
    "\n",
    "mark S with n\n",
    "while G is unmarked\n",
    "    n ← n + 1\n",
    "    for each unmarked neighbor (c) of the current frontier cells explored.\n",
    "        mark c with n\n",
    "\n",
    "current ← G\n",
    "append current to path\n",
    "while S not in path\n",
    "    append lowest marked neighbor c of current to path\n",
    "    current ← c\n",
    "\"\"\""
   ]
  }
 ],
 "metadata": {
  "hide_input": false,
  "kernelspec": {
   "display_name": "last_updat_python11_07_2025",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.13.5"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": true,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": true
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
